import sensor, image, time, math, pyb, lcd
from pyb import UART
from pyb import LED
usart3=UART(3,115200)
usart3.init(115200, bits=8, parity=None, stop=1)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_auto_exposure(False,7000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.skip_frames(time = 2000)
clock = time.clock()
LED(3).on()


Mode=0
red = (75, 100, -17, 127, -128, 127)#(35, 79, 29, 127, -128, 127)
red2=(12, 28, 30, 127, -56, 64)#(12, 88, -128, 127, -128, 127)

def UART_Out(Dian):
    data = bytearray([0xb3, 0xb3, int(Dian[0]), int(Dian[1]), int(Dian[2]), 0x5b])
    usart3.write(data)

while(True):
    clock.tick()
    flag=0
    img = sensor.snapshot().lens_corr(strength = 1.8)
    colormax=[]
    r_blobs = img.find_blobs([red], x_stride =2,y_stride =2,area_threshold =10,pixels_threshold =1 )
    if r_blobs:
        if r_blobs[0].pixels()<200:
            if r_blobs[0].roundness()>0.7:
                img.draw_rectangle(r_blobs[0].rect(),color =(255,0,0))
                print(r_blobs)
                colorxy=[r_blobs[0].cx(),r_blobs[0].cy(),0x02]
                flag=1
                UART_Out(colorxy)
    else:
        red2_blobs = img.find_blobs([red2], x_stride =2,y_stride =2,area_threshold =2,pixels_threshold =10)
        if red2_blobs:
            if red2_blobs[0].pixels()<70:
                img.draw_rectangle(red2_blobs[0].rect(),color =(0,255,0))
                print(red2_blobs)
                colorxy=[red2_blobs[0].cx(),red2_blobs[0].cy(),0x02]
                flag=1
                UART_Out(colorxy)
        else:
            red3_blobs = img.find_blobs([red2], x_stride =2,y_stride =2,area_threshold =2,pixels_threshold =1)
            if red3_blobs:
                img.draw_rectangle(red3_blobs[0].rect(),color =(0,0,255))
                print(red3_blobs)
                colorxy=[red3_blobs[0].cx(),red3_blobs[0].cy(),0x02]
                flag=1
                UART_Out(colorxy)

    if flag==0:
        colorxy=[0x01,0x01,0x05]
        UART_Out(colorxy)
    img.draw_string(0,0,"fps:%d"%(int(clock.fps())),color = (255, 255, 0), scale = 2)
